/**
* @file NavigateToPoseActionClient.h
* @author
* @brief
* @version 1.0
* @date 2021-04-26
*
* @copyright Copyright (c) 2021
*
*/

#pragma once

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <mutex>

namespace behavior_controller
{
enum class ActionStatus
{
    UNKNOWN = 0,
    PROCESSING = 1,
    FAILED = 2,
    SUCCEEDED = 3
};

class NavigateToPoseActionClient
{
public :
    using ClientT = nav2_msgs::action::NavigateToPose;

    NavigateToPoseActionClient(const rclcpp::Node::SharedPtr &ros_node, std::mutex *lock_,
                               const std::string action_name);

    void resultCallback(const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult &result);

    void goalResponseCallback(const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr &future);

    void sendGoal(geometry_msgs::msg::PoseStamped &pose_goal);

    void feedback_callback(rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr,
                           const std::shared_ptr<const ClientT::Feedback> feedback);

    void work();

    bool isFinish() { return finished; };

    void setState(ActionStatus client_goal_)
    { current_goal_status_ = client_goal_; };

    ActionStatus runState()
    { return current_goal_status_; };

    void cancelGoal()
    { is_cancel_requested = true; };

    bool getFlag() { return send_flag; };

    void setFlag(bool send_flag_)
    { send_flag = send_flag_; };
public:
    rclcpp::Node::SharedPtr ros_node_;
    std::mutex *lock;
    rclcpp_action::Client<ClientT>::SharedPtr nav_to_pose_client_;
    rclcpp_action::Client<ClientT>::SendGoalOptions send_goal_options;
    ClientT::Goal client_goal;
    double time_send = 0;
    ActionStatus current_goal_status_ = ActionStatus::UNKNOWN;
    bool finished = false;
    bool is_cancel_requested = false;
    bool send_flag = false;
};

extern std::shared_ptr<NavigateToPoseActionClient> navigate_to_pose_action_client;
}